/*
KalmanTrack.cxx

Implementation of class Seed, KalmanTrack

Author: Kun Liu, liuk@fnal.gov
Created: 10-14-2012
*/

#include <iostream>
#include <cmath>
#include <algorithm>

#include "GeomSvc.h"
#include "KalmanTrack.h"

double Seed::getMomentum(double& px, double& py, double& pz)
{
  pz = p/sqrt(1. + axz*axz + ayz*ayz);
  px = pz*axz;
  py = pz*ayz;

  return p;
}

void Seed::decideCharge()
{
  double z_ref = 1347.36;
  double x_ref = getExpPositionX(z_ref);

  if(x_ref/z_ref > axz)
    {
      charge = 1.;
    }
  else
    {
      charge = -1.;
    }
}

void Seed::decideMomentum()
{
  double z_ref = 1347.36;
  double z_0 = 0.;
  double z_bend_k = 1064.26;
  double z_bend_f = 251.4;
  double kick_f = -2.911;
  double kick_k = -0.4;
  double deltaE_kf = 8.12;
  double deltaE_absorber = 1.81;
  double c4 = deltaE_kf;
  double c5 = deltaE_kf/2.;

  double x_ref = axz*z_ref + bxz;
  
  decideCharge();

  double c1 = (z_0 - z_bend_f)*kick_f*charge;
  double c2 = (z_0 - z_bend_k)*kick_k*charge;
  double c3 = axz*(z_ref - z_0) - x_ref;
  double b = c1/c3 + c2/c3 - c4 - c5;
  double c = c4*c5 - c1*c5/c3 - c2*c4/c3;

  double disc = b*b - 4.*c;
  if(disc < 0)
    {
      decideMomentum_model();
    }
  else
    {
      p = (-b + sqrt(disc))/2.;
    }

  if(p < 0.)
    {
      decideMomentum_model();
    }
  else if(p < 15. || p > 90.)
    {
      decideMomentum_model();
    }  

  p -= (deltaE_kf + deltaE_absorber);
}

void Seed::decideMomentum_model()
{
  double z_bend = 375.;
  double x_bend = axz*z_bend + bxz;

  double kick = fabs(x_bend/z_bend - axz);
  p = 1.0/(0.00832161 + 0.184186*kick - 0.104132*kick*kick);
}

bool Seed::similarity(Seed& elem)
{
  if(charge*elem.charge < 0) return false;
  if(fabs(p - elem.p) > .2*p) return false;
  
  GeomSvc *p_geomSvc = GeomSvc::instance();
  double z = p_geomSvc->getPlanePosition(41);
  double dx = getExpPositionX(z) - elem.getExpPositionX(z);
  double dy = getExpPositionY(z) - elem.getExpPositionY(z);
  if(dx*dx + dy*dy > 100.) return false; 

  return true;
}

void Seed::reduceSeedList(std::list<Seed>& _seedlist)
{
  std::list<Seed> _targetlist;
  _targetlist.clear();

  while(!_seedlist.empty())
    {
      _targetlist.push_back(_seedlist.front());
      _seedlist.pop_front();

      for(std::list<Seed>::iterator iter = _seedlist.begin(); iter != _seedlist.end(); )
	{
	  if(iter->similarity(_targetlist.back()))
	    {
	      iter = _seedlist.erase(iter);
	      continue;
	    }

	  ++iter;
	}
    }

  _seedlist.assign(_targetlist.begin(), _targetlist.end());
}

void Seed::print()
{
  std::cout << "=========== Info. of this seed ===========" << std::endl;
  std::cout << "X-Z seed:" << axz << "*z + " << bxz << std::endl;
  std::cout << "Y-Z seed:" << ayz << "*z + " << byz << std::endl;
  std::cout << "Momentum: " << p << std::endl;
  std::cout << "Charge  : " << charge << std::endl;
}

///Default constructor for KalmanTrack, wiill only be used when resizing the track candidate list
KalmanTrack::KalmanTrack()
{
  _hit_index.clear();
  _nodes.clear();

  _chisq = 0.;
  update();
}

KalmanTrack::KalmanTrack(Seed _seed_input)
{
  _seed = _seed_input;

  _hit_index.clear();
  _nodes.clear();

  _chisq = 0.;
  update();

  GeomSvc *p_geomSvc = GeomSvc::instance();

  _trkpar_curr._z = p_geomSvc->getPlanePosition(41);
  _trkpar_curr._state_kf[0][0] = _seed.charge/_seed.p;
  _trkpar_curr._state_kf[1][0] = _seed.axz;
  _trkpar_curr._state_kf[2][0] = _seed.ayz;
  _trkpar_curr._state_kf[3][0] = _seed.getExpPositionX(_trkpar_curr._z);
  _trkpar_curr._state_kf[4][0] = _seed.getExpPositionY(_trkpar_curr._z);

  _trkpar_curr._covar_kf.Zero();
  _trkpar_curr._covar_kf[0][0] = 0.001;
  _trkpar_curr._covar_kf[1][1] = 0.01;
  _trkpar_curr._covar_kf[2][2] = 0.01;
  _trkpar_curr._covar_kf[3][3] = 100.;
  _trkpar_curr._covar_kf[4][4] = 100.; 
}

KalmanTrack::KalmanTrack(SRecTrack& _trk, SRawEvent *_rawevt, SRecEvent *_recevt)
{
  _chisq = _trk.getChisq();
  
  _hit_index.clear();
  _nodes.clear();

  GeomSvc *p_geomSvc = GeomSvc::instance();
  for(int i = 0; i < _trk.getNHits(); i++)
    {
      _hit_index.push_back(_trk.getHitIndex(i));

      ///Update the hit info. if needed
      Hit _hit = _rawevt->getHit(_recevt->getLocalID(_trk.getHitIndex(i)));
      _hit.pos = p_geomSvc->getMeasurement(_hit.detectorID, _hit.elementID);
      _hit.driftDistance = _hit.getSign()*p_geomSvc->getDriftDistance(_hit.detectorID, _hit.tdcTime);

      Node _node(_hit);
      _node.setZ(_trk.getZ(i));
      _node.setChisq(_trk.getChisqAtNode(i));
      _node.getPredicted()._state_kf = _trk.getStateVector(i);
      _node.getPredicted()._covar_kf = _trk.getCovariance(i);
      _node.getPredicted()._z = _trk.getZ(i);
      _node.getFiltered()._state_kf = _trk.getStateVector(i);
      _node.getFiltered()._covar_kf = _trk.getCovariance(i);
      _node.getFiltered()._z = _trk.getZ(i);
      _node.getSmoothed()._state_kf = _trk.getStateVector(i);
      _node.getSmoothed()._covar_kf = _trk.getCovariance(i);
      _node.getSmoothed()._z = _trk.getZ(i);
      _node.setSmoothDone();
      _node.setPredictionDone();
      _node.setFilterDone();
   }
}

bool KalmanTrack::isValid()
{
  if(_chisq < 0 || _chisq > 150) return false;
  if(_nodes.empty()) return false;
  if(getMomentumUpstream() < 15. || getMomentumUpstream() > 105.) return false;

  return true;
}

int KalmanTrack::isInStation3()
{
  GeomSvc* p_geomSvc = GeomSvc::instance();
  
  int detectorID_st3p = 18;
  //int detectorID_st3m = 24;
  double z_st3p = p_geomSvc->getPlanePosition(detectorID_st3p);
  //double z_st3m = p_geomSvc->getPlanePosition(detectorID_st3m);

  double x_st3p = _seed.getExpPositionX(z_st3p);
  double y_st3p = _seed.getExpPositionY(z_st3p);
  if(p_geomSvc->isInPlane(detectorID_st3p, x_st3p, y_st3p))
    {
      return 1;
    }
  else
    {
      return -1;
    }
}

void KalmanTrack::updateMomentum()
{
  double z_0 = 0.;
  double z_bend_k = 1064.26;
  double z_bend_f = 251.4;
  double kick_f = -2.911;
  double kick_k = -0.4;
  double deltaE_kf = 8.12;
  double c4 = deltaE_kf;
  double c5 = deltaE_kf/2.;

  double z_ref = _nodes.front().getZ();
  double x_ref = _nodes.front().getFiltered()._state_kf[3][0];
  double axz = _nodes.front().getFiltered()._state_kf[1][0];

  double charge;
  if(_nodes.front().getFiltered()._state_kf[0][0] > 0)
    {
      charge = 1.;
    }
  else
    {
      charge = -1.;
    }

  double c1 = (z_0 - z_bend_f)*kick_f*charge;
  double c2 = (z_0 - z_bend_k)*kick_k*charge;
  double c3 = axz*(z_ref - z_0) - x_ref;
  double b = c1/c3 + c2/c3 - c4 - c5;
  double c = c4*c5 - c1*c5/c3 - c2*c4/c3;

  double disc = b*b - 4.*c;
  if(disc < 0)
    {
      return;
    }

  double p = (-b + sqrt(disc))/2.;
  p -= deltaE_kf;
  
  if(p < 0. || p > 100.)
    {
      return;
    }
    
  //Log("Momentum updated from " << 1./fabs(_trkpar_curr._state_kf[0][0]) << " to " << p);
  _trkpar_curr._state_kf[0][0] = charge/p;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      iter->getPredicted()._state_kf[0][0] = charge/p;
      iter->getFiltered()._state_kf[0][0] = charge/p;
    }
}

void KalmanTrack::flipCharge()
{
  /*
  _trkpar_curr.flip_charge();
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      if(iter->isPredictionDone()) iter->getPredicted().flip_charge();
      if(iter->isFilterDone()) iter->getFiltered().flip_charge();
      if(iter->isSmoothDone()) iter->getSmoothed().flip_charge();
    }
  */

  TrkPar _trkpar = _nodes.back().getSmoothed();
  _trkpar.flip_charge();

  _trkpar._covar_kf.Zero();
  _trkpar._covar_kf[0][0] = 0.001;
  _trkpar._covar_kf[1][1] = 0.01;
  _trkpar._covar_kf[2][2] = 0.01;
  _trkpar._covar_kf[3][3] = 100.;
  _trkpar._covar_kf[4][4] = 100.;

  _nodes.back().getPredicted() = _trkpar;
}

bool KalmanTrack::propagateTo(int detectorID)
{
  Hit hit_dummy;
  hit_dummy.detectorID = detectorID;

  _node_next = Node(hit_dummy);

  KalmanFilter *kmfit = KalmanFilter::instance();
  kmfit->setCurrTrkpar(_trkpar_curr);
  if(kmfit->predict(_node_next))
    {
      return true;
    }
  else
    {
      return false;
    }
}

double KalmanTrack::getMomentumVertex(double z, double& px, double& py, double& pz)
{
  Node _node_vertex;
  _node_vertex.setZ(z);

  TMatrixD m(2, 1), cov(2, 2),  proj(2, 5);
  m[0][0] = 0.;
  m[1][0] = 0.;

  cov.Zero();
  cov[0][0] = 1.;
  cov[1][1] = 1.;

  proj.Zero();
  proj[0][3] = 1.;
  proj[1][4] = 1.;

  _node_vertex.getMeasurement().ResizeTo(2, 1);
  _node_vertex.getMeasurementCov().ResizeTo(2, 2);
  _node_vertex.getProjector().ResizeTo(2, 5);

  _node_vertex.getMeasurement() = m;
  _node_vertex.getMeasurementCov() = cov;
  _node_vertex.getProjector() = proj;

  KalmanFilter *kmfit = KalmanFilter::instance();
  kmfit->setCurrTrkpar(_nodes.front().getSmoothed());
  kmfit->fit_node(_node_vertex);

  _chisq_vertex = _node_vertex.getChisq();

  return _node_vertex.getFiltered().get_mom(px, py, pz);
  //return _node_vertex.getPredicted().get_mom(px, py, pz);
}

double KalmanTrack::getExpPosition()
{
  const TMatrixD& proj = _node_next.getProjector();
  const TMatrixD& state = _node_next.getPredicted()._state_kf;

#ifdef _DEBUG_ON
  Log("Expected X: " << _node_next.getPredicted()._state_kf[3][0]);
  Log("Expected Y: " << _node_next.getPredicted()._state_kf[4][0]);
#endif

  return (proj*state)[0][0];
}

double KalmanTrack::getExpPosError()
{
  const TMatrixD& proj = _node_next.getProjector();
  const TMatrixD& covar = _node_next.getPredicted()._covar_kf;
  //const TMatrixD& cov_m = _node_next.getMeasurementCov();

  //double err_x = sqrt(covar[3][3]);
  //double err_y = sqrt(covar[4][4]);
  //double track_err = fabs(proj[0][3]*err_x) + fabs(proj[0][4]*err_y);

  double track_err_sq = SMatrix::getABCt(proj, covar, proj)[0][0];
  double wire_spacing = GeomSvc::instance()->getPlaneSpacing(_node_next.getHit().detectorID);

  //Log("measurement error: " << sqrt(cov_m[0][0]) << ", tracking error: " << track_err);
  return sqrt(wire_spacing*wire_spacing + track_err_sq);
}

Node* KalmanTrack::getNearestNodePtr(double z)
{
  Node *_node_prev = &(_nodes.front());
  double deltaZ_curr, deltaZ_prev;
  deltaZ_prev = 1E6;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      deltaZ_curr = fabs(z - iter->getZ());
      if(deltaZ_curr > deltaZ_prev)
	{
	  return _node_prev;
	}

      deltaZ_prev = deltaZ_curr;
      _node_prev = &(*iter);
    }

  return _node_prev;
}

void KalmanTrack::getExpPositionFast(double z, double& x, double& y, Node* _node)
{
  if(_node == NULL)
    {
      _node = getNearestNodePtr(z);
    }

  TrkPar *_trkpar;
  if(_node->isSmoothDone())
    {
      _trkpar = &(_node->getSmoothed());
    }
  else if(_node->isFilterDone())
    {
      _trkpar = &(_node->getFiltered());
    }
  else 
    {
      _trkpar = &(_node->getPredicted());
    }

  double z_ref = _trkpar->get_z();
  double x_ref = _trkpar->get_x();
  double y_ref = _trkpar->get_y();
  double axz = _trkpar->get_dxdz();
  double ayz = _trkpar->get_dydz();

  x = x_ref + axz*(z - z_ref);
  y = y_ref + ayz*(z - z_ref);
}

void KalmanTrack::getExpPosErrorFast(double z, double& dx, double& dy, Node* _node)
{  
  if(_node == NULL)
    {
      _node = getNearestNodePtr(z);
    }

  TrkPar *_trkpar;
  if(_node->isSmoothDone())
    {
      _trkpar = &(_node->getSmoothed());
    }
  else if(_node->isFilterDone())
    {
      _trkpar = &(_node->getFiltered());
    }
  else 
    {
      _trkpar = &(_node->getPredicted());
    }

  double z_ref = _trkpar->get_z();
  double dx_ref = sqrt(fabs(_trkpar->_covar_kf[3][3]));
  double dy_ref = sqrt(fabs(_trkpar->_covar_kf[4][4]));
  double daxz = sqrt(fabs(_trkpar->_covar_kf[1][1]));
  double dayz = sqrt(fabs(_trkpar->_covar_kf[2][2]));

  dx = 2.*(dx_ref + fabs(daxz*(z - z_ref)));
  dy = 2.*(dy_ref + fabs(dayz*(z - z_ref)));
}

double KalmanTrack::getExpLocalSlop()
{
  const TMatrixD& proj = _node_next.getProjector();
  const TMatrixD& state = _node_next.getPredicted()._state_kf;


  return proj[0][3]*state[1][0] + proj[0][4]*state[2][0];
}

double KalmanTrack::getExpLcSlopErr()
{
  const TMatrixD& proj = _node_next.getProjector();
  const TMatrixD& covar = _node_next.getPredicted()._covar_kf;

  double err_x = sqrt(covar[1][1]);
  double err_y = sqrt(covar[2][2]);

  return fabs(proj[0][3]*err_x) + fabs(proj[0][4]*err_y);
}

Node *KalmanTrack::getNodeUpstream()
{
  Node* _node = NULL;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      int detectorID = iter->getHit().detectorID;
      if(detectorID <= 6)
	{
	  _node = &(*iter);
	}
      else
	{
	  break;
	}
    }

  return _node;
}

Node *KalmanTrack::getNodeDownstream()
{
  Node* _node = NULL;
  for(std::list<Node>::reverse_iterator iter = _nodes.rbegin(); iter != _nodes.rend(); ++iter)
    {
      int detectorID = iter->getHit().detectorID;
      if(detectorID > 6)
	{
	  _node = &(*iter);
	}
      else
	{
	  break;
	}
    }
  
  return _node;
}

double KalmanTrack::getMomentumInStation(int stationID)
{
  int detectorID_begin = stationID*6 - 5;
  int detectorID_end = stationID*6;

  double p = -1.;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      double detectorID = iter->getHit().detectorID;
      if(detectorID >= detectorID_begin && detectorID <= detectorID_end)
	{
	  p = iter->getFiltered().get_mom();
	  break;
	}
    }

  return p;
}

double KalmanTrack::getXZSlopeInStation(int stationID)
{
  int detectorID_begin = stationID*6 - 5;
  int detectorID_end = stationID*6;

  double axz = -100.;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      double detectorID = iter->getHit().detectorID;
      if(detectorID >= detectorID_begin && detectorID <= detectorID_end)
	{
	  axz = iter->getFiltered().get_dxdz();
	  break;
	}
    }

  return axz;
}

double KalmanTrack::getPositionInStation(int stationID, double& x, double& y, double& z)
{
  int detectorID_begin = stationID*6 - 5;
  int detectorID_end = stationID*6;

  x = 9999;
  y = 9999;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      double detectorID = iter->getHit().detectorID;
      if(detectorID >= detectorID_begin && detectorID <= detectorID_end)
	{
	  z = iter->getZ();
	  if(iter->isSmoothDone())
	    {
	      x = iter->getSmoothed().get_x();
	      y = iter->getSmoothed().get_y();
	    }
	  else if(iter->isFilterDone())
	    {
	      x = iter->getFiltered().get_x();
	      y = iter->getFiltered().get_y();
	    }
	  break;
	}
    }

  return sqrt(x*x + y*y);

}

int KalmanTrack::getKickCharge()
{
  double axz_st1 = getXZSlopeInStation(1);
  double axz_st2 = getXZSlopeInStation(2);

  return axz_st1 < axz_st2 ? -1 : 1;
}

double KalmanTrack::getMomentumUpstream(double& px, double& py, double& pz)
{
  if(_nodes.front().isSmoothDone())
    {
      return _nodes.front().getSmoothed().get_mom(px, py, pz);
    }
  else if(_nodes.front().isFilterDone())
    {
      return _nodes.front().getFiltered().get_mom(px, py, pz);
    }
  else
    {
      return _nodes.front().getPredicted().get_mom(px, py, pz);
    }
}

int KalmanTrack::getNHitsInStation(int stationID)
{
  int detectorID_begin = stationID*6 - 5;
  int detectorID_end = stationID*6;

  int nHits = 0;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      int detectorID = iter->getHit().detectorID;
      if(detectorID >= detectorID_begin && detectorID <= detectorID_end)
	{
	  nHits++;
	}
    }

  return nHits;
}

void KalmanTrack::update()
{
  double nHits = _hit_index.size();

  _chisq = 0.;
  for(std::list<Node>::iterator node = _nodes.begin(); node != _nodes.end(); ++node)
    {
      _chisq += node->getChisq();
    }

  _quality = nHits - 0.4*_chisq;
}

bool KalmanTrack::operator<(const KalmanTrack& elem) const
{
  return _quality > elem._quality;
}

bool KalmanTrack::operator==(const KalmanTrack& elem) const
{
  if(_hit_index.size() != elem._hit_index.size()) return false;
  if(fabs(_quality - elem._quality) > 1E-6) return false;

  return true;
}

bool KalmanTrack::similarity(const KalmanTrack& elem) const
{
  std::list<int> _hit_index1, _hit_index2;

  _hit_index1.clear();
  for(std::list<int>::const_iterator iter = _hit_index.begin(); iter != _hit_index.end(); ++iter)
    {
      _hit_index1.push_back(abs(*iter));
    }
  _hit_index1.sort();

  _hit_index2.clear();
  for(std::list<int>::const_iterator iter = elem._hit_index.begin(); iter != elem._hit_index.end(); ++iter)
    {
      _hit_index2.push_back(abs(*iter));
    }
  _hit_index2.sort();

  std::list<int> commonHits;
  commonHits.clear();
  set_intersection(_hit_index1.begin(), _hit_index1.end(), _hit_index2.begin(), _hit_index2.end(), back_inserter(commonHits));

  double nHits_original = double(_hit_index1.size());
  double nHits_common = double(commonHits.size());

  if(nHits_common/nHits_original > 0.2) return true;
  return false;
}


bool KalmanTrack::addHit(Hit _hit)
{
  _hit_index.push_front(_hit.index);
  
  Node _node(_hit);
  KalmanFilter *_kmfit = KalmanFilter::instance();

  _node.getPredicted() = _node_next.getPredicted();
  _node.setPredictionDone();
  if(_kmfit->filter(_node))
    {
      _nodes.push_front(_node);
      update();
    
      _trkpar_curr = _node.getFiltered();
        
      return true;
    }
  else
    {
      return false;
    }
}

int KalmanTrack::getAlignment(int level, int *detectorID, double *res, double *R, double *T)
{
  if(level != 1 && level != 2 && level != 3)
    {
      std::cerr << "Wrong output level selection! " << std::endl;
      return 0;
    }

  int nHits = 0;
  double x, y;

  GeomSvc *p_geomSvc = GeomSvc::instance();
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      if(level == 3)
	{
	  x = iter->getSmoothed().get_x();
	  y = iter->getSmoothed().get_y();
	  res[nHits] = iter->getSmoothedResidual()[0][0];
	}
      else if(level == 2)
	{
	  x = iter->getFiltered().get_x();
	  y = iter->getFiltered().get_y();
	  res[nHits] = iter->getFilteredResidual()[0][0];
	}
      else if(level == 1)
	{
	  x = iter->getPredicted().get_x();
	  y = iter->getPredicted().get_y();
	  res[nHits] = iter->getPredictedResidual()[0][0];
	}

      detectorID[nHits] = iter->getHit().detectorID;
      R[nHits] = x*p_geomSvc->getCostheta(detectorID[nHits]) + y*p_geomSvc->getSintheta(detectorID[nHits]) - iter->getHit().pos;
      T[nHits] = iter->getHit().tdcTime;

      nHits++;
    }

  return nHits;
}

int KalmanTrack::getPositions(int level, double *x, double *y, double *z)
{
  if(level != 1 && level != 2 && level != 3)
    {
      std::cerr << "Wrong output level selection! " << std::endl;
      return 0;
    }

  int nHits = 0;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      z[nHits] = iter->getZ();
      if(level == 3)
	{
	  x[nHits] = iter->getSmoothed().get_x();
	  y[nHits] = iter->getSmoothed().get_y();
	}
      else if(level == 2)
	{
	  x[nHits] = iter->getFiltered().get_x();
	  y[nHits] = iter->getFiltered().get_y();
	}
      else if(level == 1)
	{
	  x[nHits] = iter->getPredicted().get_x();
	  y[nHits] = iter->getPredicted().get_y();
	}

      nHits++;
    }

  return nHits;
}

int KalmanTrack::getMomentums(int level, double *px, double *py, double *pz)
{
  if(level != 1 && level != 2 && level != 3)
    {
      std::cerr << "Wrong output level selection! " << std::endl;
      return 0;
    }

  int nHits = 0;
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      double m_px, m_py, m_pz;
      if(level == 3)
	{
	  iter->getSmoothed().get_mom(m_px, m_py, m_pz);
	}
      else if(level == 2)
	{
	  iter->getFiltered().get_mom(m_px, m_py, m_pz);
	}
      else if(level == 1)
	{
	  iter->getPredicted().get_mom(m_px, m_py, m_pz);
	}

      px[nHits] = m_px;
      py[nHits] = m_py;
      pz[nHits] = m_pz;

      nHits++;
    }

  return nHits;
}

double KalmanTrack::getMuIDAngle()
{
  double z_absorber = 2000.;
  double z_proptube = 3000.;
  
  double x_seed = _seed.getExpPositionX(z_proptube);
  double y_seed = _seed.getExpPositionY(z_proptube);
  double x_trk, y_trk;
  getExpPositionFast(z_proptube, x_trk, y_trk);

  double angle = sqrt((x_trk - x_seed)*(x_trk - x_seed) + (y_trk - y_seed)*(y_trk - y_seed))/(z_proptube - z_absorber);
  return angle;
}

int KalmanTrack::getHitsIndex(int *index)
{
  int nHits = 0;
  for(std::list<int>::iterator iter = _hit_index.begin(); iter != _hit_index.end(); ++iter)
    {
      index[nHits] = *iter;
      nHits++;
    }

  return nHits;
}

std::vector<int> KalmanTrack::getMissedDetectorIDs()
{
  std::vector<int> detectorIDs_all;
  for(int i = 1; i <= 12; ++i) detectorIDs_all.push_back(i);
  if(_nodes.back().getHit().detectorID < 19)
    {
      for(int i = 13; i <= 18; ++i) detectorIDs_all.push_back(i);
    }
  else
    {
      for(int i = 19; i <= 24; ++i) detectorIDs_all.push_back(i);
    }

  std::vector<int> detectorIDs_now;
  detectorIDs_now.clear();
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      detectorIDs_now.push_back(iter->getHit().detectorID);
    }
  std::sort(detectorIDs_now.begin(), detectorIDs_now.end());

  std::vector<int> detectorIDs_miss(18);
  std::vector<int>::iterator iter = std::set_difference(detectorIDs_all.begin(), detectorIDs_all.end(), detectorIDs_now.begin(), detectorIDs_now.end(), detectorIDs_miss.begin());
  detectorIDs_miss.resize(iter - detectorIDs_miss.begin());

  return detectorIDs_miss;
}

void KalmanTrack::getSagittaInSuperDetector(int detectorID, double& pos_exp, double& window)
{
  if(detectorID > 3 || detectorID < 1) return;
  GeomSvc *p_geomSvc = GeomSvc::instance();

  ///DetectorID of the relevant view of D1U, D1X and D1V
  int detectorID_st2[3] = {12, 10, 8};
  double ratio[3] = {1.9, 1.80, 1.7};
  double sigma[3] = {0.2, 0.2, 0.2};

  double x_st3, y_st3, z_st3;
  z_st3 = _nodes.back().getZ();
  if(_nodes.back().isSmoothDone())
    {
      x_st3 = _nodes.back().getSmoothed().get_x();
      y_st3 = _nodes.back().getSmoothed().get_y();
    }
  else if(_nodes.back().isFilterDone())
    {
      x_st3 = _nodes.back().getFiltered().get_x();
      y_st3 = _nodes.back().getFiltered().get_y();
    }
  double pos_st3 = x_st3*p_geomSvc->getCostheta(_nodes.back().getHit().detectorID) + y_st3*p_geomSvc->getSintheta(_nodes.back().getHit().detectorID);

  double x_st2, y_st2, z_st2;
  z_st2 = p_geomSvc->getPlanePosition(detectorID_st2[detectorID-1]);
  getExpPositionFast(z_st2, x_st2, y_st2);
  double pos_st2 = x_st2*p_geomSvc->getCostheta(detectorID_st2[detectorID-1]) + y_st2*p_geomSvc->getSintheta(detectorID_st2[detectorID-1]);
  double sagitta_st2 = pos_st2 - pos_st3/z_st3*z_st2;

  double z_st1 = p_geomSvc->getPlanePosition(2*detectorID);
  pos_exp =  sagitta_st2*ratio[detectorID-1] + pos_st3/z_st3*z_st1; 
  window = fabs(6.*sagitta_st2*sigma[detectorID-1]);
}

TGraph KalmanTrack::getXZProjection()
{
  double x[20], y[20], z[20];
  getPositions(3, x, y, z);

  TGraph gr(getNHits(), z, x);
  return gr;
}

SRecTrack KalmanTrack::getSRecTrack()
{
  SRecTrack _strack;

  _strack.setChisq(_chisq);
  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      _strack.insertHitIndex(iter->getHit().index);
      _strack.insertStateVector(iter->getSmoothed()._state_kf);
      _strack.insertCovariance(iter->getSmoothed()._covar_kf);
      _strack.insertZ(iter->getZ());
      _strack.insertChisq(iter->getChisq());
    }

  return _strack;
}

void KalmanTrack::print()
{
  std::cout << "=============== Kalman finder candidate track ==================" << std::endl;
  std::cout << "This candidate has " << _hit_index.size() << " hits!" << std::endl;

  for(std::list<int>::iterator iter = _hit_index.begin(); iter != _hit_index.end(); ++iter)
    {
      std::cout << *iter << " : ";
    }
  std::cout << std::endl;

  for(std::list<Node>::iterator iter = _nodes.begin(); iter != _nodes.end(); ++iter)
    {
      std::cout << iter->getHit().detectorID << " : ";
    }
  std::cout << std::endl;

  std::cout << "Most upstream momentum is: " << 1./fabs(_trkpar_curr._state_kf[0][0]) << std::endl;
  std::cout << "Most upstream position is: " << _trkpar_curr.get_z() << std::endl;
  std::cout << "Current slope:    X-Z = " << _trkpar_curr._state_kf[1][0] << ", Y-Z = " << _trkpar_curr._state_kf[2][0] << std::endl;
  std::cout << "Current position: X = " << _trkpar_curr._state_kf[3][0] << ", Y = " << _trkpar_curr._state_kf[4][0] << std::endl;
  std::cout << "Quality is: " << _quality << std::endl;
  std::cout << "Chisq/d.o.f = " << _chisq/_hit_index.size() << std::endl;
  if(!_hit_index.empty()) std::cout << "Charge      = " << getCharge() << std::endl;
  if(!_hit_index.empty()) std::cout << "Assigned Charge  = " << getAssignedCharge() << std::endl;
  if(!_hit_index.empty()) std::cout << "Kick Charge      = " << getKickCharge() << std::endl;

  //std::cout << "Using this seed as start: " << std::endl;
  //_seed.print();
}

void KalmanTrack::printNodes()
{
  std::cout << "The content of all Nodes of this track ... " << std::endl;
  for(std::list<Node>::reverse_iterator node = _nodes.rbegin(); node != _nodes.rend(); ++node)
    {
      node->print(true);
    }
}
